/*
 * Copyright (c) 2006-2020, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2021-03-10     CGY       the first version
 */
#include "head_rtthread.h"
#include "head_user.h"
#include "math.h"

#define Motor4Matrix(x, y, yaw) (x*pid[Rol_Rate].OutPut/500.0f + \
                                 y*pid[Pit_Rate].OutPut/500.0f + \
                                 yaw*pid[Yaw_Rate].OutPut/500.0f)

#define StableMatrix(x, y) (x*pid[stableRol].OutPut/500.0f + \
                            y*pid[stablePit].OutPut/500.0f)

#define get_cos(x) cosf((x-500)*0.001571)    // 1/500.0f*45/57.3f
/*===================================================================*/
    /* external interface */
    MT_HandleTypedef MT_Handle;
/*===================================================================*/
    static void Motor_state(MTState state)
    {
        PID_Struct *pid = PID_Handle.pid;

        switch ( state )
        {
            case Moter_lock:
            {
                if(RC_Handle.Flyflag.Ready)
                {
                    MT_Handle.Motor[Motor_m1] = 0;
                    MT_Handle.Motor[Motor_m2] = 0;
                    MT_Handle.Motor[Motor_m3] = 0;
                    MT_Handle.Motor[Motor_m4] = 0;
                }
                else
                {
                    MT_Handle.Motor[Motor_m1] = -500;
                    MT_Handle.Motor[Motor_m2] = -500;
                    MT_Handle.Motor[Motor_m3] = -500;
                    MT_Handle.Motor[Motor_m4] = -500;
                }
                MT_Handle.Motor[Motor_m5] = 400;
                MT_Handle.Motor[Motor_m6] = 400;
                MT_Handle.Motor[Motor_m7] = 400;
                MT_Handle.Motor[Motor_m8] = 400;

                PID_Handle.pidReset2();
            }break;
            case Moter_lazy:
            {
                MT_Handle.Motor[Motor_m1] = lazy_speed;
                MT_Handle.Motor[Motor_m2] = lazy_speed;
                MT_Handle.Motor[Motor_m3] = lazy_speed;
                MT_Handle.Motor[Motor_m4] = lazy_speed;
                MT_Handle.Motor[Motor_m5] = 500;
                MT_Handle.Motor[Motor_m6] = 500;
                MT_Handle.Motor[Motor_m7] = 500;
                MT_Handle.Motor[Motor_m8] = 500;

                PID_Handle.pidReset2();
            }break;
            case Moter_obey:
            {
                /*===================================================================*/
                if( RC_Handle.Flyflag.Height_ctrl == acce_mode )
                {
                    MT_Handle.base_out  = RC_Handle.rc_dat[THR] + LPF_1[LPF_motor_hover].output;
                }
                else if( RC_Handle.Flyflag.Height_ctrl == sped_mode )
                {
                    /* 悬停油门量低通滤波在  遥控器给飞控上锁时  复位,借此获得更好的起飞体验 (后面俩数据给前面赋值之后被清零，会出现打印数据时前后数据不照应的情况，对飞行体验一点没影响)*/
                    MT_Handle.base_out  = (uint16_t)(limit(pid[Z_Speed].OutPut*0.8f,-400,400) + LPF_1[LPF_motor_hover].output);   //速度输出加悬停油门
                }
                /* 四旋翼 */
                static uint16_t S1, S2, S3, S4;
                S1 = limit(500 + 500*Motor4Matrix(+0.8, +0.8, 0.6), 0, 1000);
                S2 = limit(500 + 500*Motor4Matrix(-0.8, -0.8, 0.6), 0, 1000);
                S3 = limit(500 + 500*Motor4Matrix(+0.8, -0.8, 0.6), 0, 1000);
                S4 = limit(500 + 500*Motor4Matrix(-0.8, +0.8, 0.6), 0, 1000);

                MT_Handle.Motor[Motor_m5] += 0.3*(S1 - MT_Handle.Motor[Motor_m5]);
                MT_Handle.Motor[Motor_m6] += 0.3*(S2 - MT_Handle.Motor[Motor_m6]);
                MT_Handle.Motor[Motor_m7] += 0.3*(S3 - MT_Handle.Motor[Motor_m7]);
                MT_Handle.Motor[Motor_m8] += 0.3*(S4 - MT_Handle.Motor[Motor_m8]);

//                MT_Handle.Motor[Motor_m5] = limit(500 + 500*Motor4Matrix(+0.8, +0.8, 0.6), 0, 1000);
//                MT_Handle.Motor[Motor_m6] = limit(500 + 500*Motor4Matrix(-0.8, -0.8, 0.6), 0, 1000);
//                MT_Handle.Motor[Motor_m7] = limit(500 + 500*Motor4Matrix(+0.8, -0.8, 0.6), 0, 1000);
//                MT_Handle.Motor[Motor_m8] = limit(500 + 500*Motor4Matrix(-0.8, +0.8, 0.6), 0, 1000);

                float tmp = 0;
                tmp  = get_cos(MT_Handle.Motor[Motor_m5]);
                tmp += get_cos(MT_Handle.Motor[Motor_m6]);
                tmp += get_cos(MT_Handle.Motor[Motor_m6]);
                tmp += get_cos(MT_Handle.Motor[Motor_m8]);
                tmp  = 4.0f/tmp;

                MT_Handle.Motor[Motor_m1] = limit(MT_Handle.base_out + 150*StableMatrix(-1,   1), 0, 1000);
                MT_Handle.Motor[Motor_m2] = limit(MT_Handle.base_out + 150*StableMatrix( 1,  -1), 0, 1000);
                MT_Handle.Motor[Motor_m3] = limit(MT_Handle.base_out + 150*StableMatrix( 1,   1), 0, 1000);
                MT_Handle.Motor[Motor_m4] = limit(MT_Handle.base_out + 150*StableMatrix(-1,  -1), 0, 1000);

//                MT_Handle.Motor[Motor_m1] = 0;
//                MT_Handle.Motor[Motor_m2] = 0;
//                MT_Handle.Motor[Motor_m3] = 0;
//                MT_Handle.Motor[Motor_m4] = 0;
                /*===================================================================*/
            }break;
            case Motor_ESCCali:
            {
                MT_Handle.Motor[Motor_m1] = RC_Handle.rc_ori[THR]-1000;
                MT_Handle.Motor[Motor_m2] = RC_Handle.rc_ori[THR]-1000;
                MT_Handle.Motor[Motor_m3] = RC_Handle.rc_ori[THR]-1000;
                MT_Handle.Motor[Motor_m4] = RC_Handle.rc_ori[THR]-1000;
                MT_Handle.Motor[Motor_m5] = 500;
                MT_Handle.Motor[Motor_m6] = 500;
                MT_Handle.Motor[Motor_m7] = 500;
                MT_Handle.Motor[Motor_m8] = 500;
            }break;
            default:break;
        }
        /*===================================================================*/
        /* final output */

        /* motor different correct */
//        MT_Handle.Motor[Motor_m3] += 20;       //-:下 +:上
//        MT_Handle.Motor[Motor_m4] += 110;      //-:上 +:下

        Set_pwm(MT_Handle.Motor);
        /*===================================================================*/
    }


#if 0
static void Motor_state(MTState state)
{
    PID_Struct *pid = PID_Handle.pid;

    switch ( state )
    {
        case Moter_lock:
        {
            MT_Handle.Motor[Motor_m1] = 0;
            MT_Handle.Motor[Motor_m2] = 0;
            MT_Handle.Motor[Motor_m3] = 380;
            MT_Handle.Motor[Motor_m4] = 380;

            PID_Handle.pidReset2();
        }break;
        case Moter_lazy:
        {
            MT_Handle.Motor[Motor_m1] = lazy_speed;
            MT_Handle.Motor[Motor_m2] = lazy_speed;
            MT_Handle.Motor[Motor_m3] = 500;
            MT_Handle.Motor[Motor_m4] = 500;

            PID_Handle.pidReset2();
        }break;
        case Moter_obey:
        {
#if 0
        /*===================================================================*/
        if( RC_Handle.Flyflag.Height_ctrl == acce_mode )
        {
            MT_Handle.base_out  = RC_Handle.rc_dat[THR] + LPF_1[LPF_motor_hover].output;
        }
        else if( RC_Handle.Flyflag.Height_ctrl == sped_mode )
        {
            /* 悬停油门量低通滤波在  遥控器给飞控上锁时  复位,借此获得更好的起飞体验 (后面俩数据给前面赋值之后被清零，会出现打印数据时前后数据不照应的情况，对飞行体验一点没影响)*/
            MT_Handle.base_out  = (uint16_t)(limit(pid[Z_Speed].OutPut*0.8f,-400,400) + LPF_1[LPF_motor_hover].output);   //速度输出加悬停油门
        }
        /* 双旋翼 */
        MT_Handle.Motor[Motor_m3] = 500 + pid[Pit_Rate].OutPut*(-0.8f) + pid[Yaw_Rate].OutPut*( 0.4f);
        MT_Handle.Motor[Motor_m4] = 500 + pid[Pit_Rate].OutPut*( 0.8f) + pid[Yaw_Rate].OutPut*( 0.4f);
        /* 悬停 */
        MT_Handle.Motor[Motor_m1] = lazy_speed + MT_Handle.base_out;
        MT_Handle.Motor[Motor_m2] = lazy_speed + MT_Handle.base_out;
        /* 偏航补偿 */
        MT_Handle.Motor[Motor_m1] = 1.0f/cosf(fabsf(pid[Yaw_Rate].OutPut/200.0f)/57.3f)*MT_Handle.Motor[Motor_m1]*(1.0f/cosf(fabsf(pid[Pit_Rate].OutPut/200.0f)/57.3f));
        MT_Handle.Motor[Motor_m2] = 1.0f/cosf(fabsf(pid[Yaw_Rate].OutPut/200.0f)/57.3f)*MT_Handle.Motor[Motor_m2]*(1.0f/cosf(fabsf(pid[Pit_Rate].OutPut/200.0f)/57.3f));
        /* 加入横滚 */
        MT_Handle.Motor[Motor_m1] += pid[Rol_Rate].OutPut*( 0.4f);
        MT_Handle.Motor[Motor_m2] += pid[Rol_Rate].OutPut*(-0.4f);
        /*===================================================================*/




#else
        /*===================================================================*/
        if( RC_Handle.Flyflag.Height_ctrl == acce_mode )
        {
            MT_Handle.base_out  = RC_Handle.rc_dat[THR] + LPF_1[LPF_motor_hover].output;
        }
        else if( RC_Handle.Flyflag.Height_ctrl == sped_mode )
        {
            /* 悬停油门量低通滤波在  遥控器给飞控上锁时  复位,借此获得更好的起飞体验 (后面俩数据给前面赋值之后被清零，会出现打印数据时前后数据不照应的情况，对飞行体验一点没影响)*/
            MT_Handle.base_out  = (uint16_t)(limit(pid[Z_Speed].OutPut*0.8f,-400,400) + LPF_1[LPF_motor_hover].output);   //速度输出加悬停油门
        }
        /* 双旋翼 */
        MT_Handle.Motor[Motor_m3] = 500 + pid[Pit_Rate].OutPut*( 1.0f) + pid[Yaw_Rate].OutPut*(1.0f);
        MT_Handle.Motor[Motor_m4] = 500 + pid[Pit_Rate].OutPut*(-1.0f) + pid[Yaw_Rate].OutPut*(1.0f);

        /* 悬停 */
        MT_Handle.Motor[Motor_m1] = lazy_speed + MT_Handle.base_out;
        MT_Handle.Motor[Motor_m2] = lazy_speed + MT_Handle.base_out;
        /* 偏航补偿 */
        MT_Handle.Motor[Motor_m1] = 1.0f/cosf(fabsf(pid[Yaw_Rate].OutPut/200.0f)/57.3f)*MT_Handle.Motor[Motor_m1]*(1.0f/cosf(fabsf(pid[Pit_Rate].OutPut/125.0f)/57.3f));
        MT_Handle.Motor[Motor_m2] = 1.0f/cosf(fabsf(pid[Yaw_Rate].OutPut/200.0f)/57.3f)*MT_Handle.Motor[Motor_m2]*(1.0f/cosf(fabsf(pid[Pit_Rate].OutPut/125.0f)/57.3f));
        /* 加入横滚 */
        MT_Handle.Motor[Motor_m1] += pid[Rol_Rate].OutPut*(-0.8f);
        MT_Handle.Motor[Motor_m2] += pid[Rol_Rate].OutPut*( 0.8f);

        /* 加入横滚 */
        MT_Handle.Motor[Motor_m1] = limit(MT_Handle.Motor[Motor_m1], 0, 1000);
        MT_Handle.Motor[Motor_m2] = limit(MT_Handle.Motor[Motor_m2], 0, 1000);
        /*===================================================================*/
#endif
        }break;
        case Motor_ESCCali:
        {
            MT_Handle.Motor[Motor_m1] = RC_Handle.rc_ori[THR]-1000;
            MT_Handle.Motor[Motor_m2] = RC_Handle.rc_ori[THR]-1000;
            MT_Handle.Motor[Motor_m3] = RC_Handle.rc_ori[THR]-1000;
            MT_Handle.Motor[Motor_m4] = RC_Handle.rc_ori[THR]-1000;
        }break;
        default:break;
    }
    /*===================================================================*/
    /* final output */

    /* motor different correct */
    MT_Handle.Motor[Motor_m3] += 20;       //-:下 +:上
    MT_Handle.Motor[Motor_m4] += 110;      //-:上 +:下

    Set_pwm(MT_Handle.Motor);
    /*===================================================================*/
}
#endif

/* 舵机：正逆负顺 */
static int MT_init(void)
{
    MT_Handle.stateUpdate = Motor_state;

    return 0;
}

INIT_ENV_EXPORT(MT_init);
